#!/usr/bin/python
# -*- coding: UTF-8 -*-
from ctypes import *
import os

# lib_name = "/home/avatar/minibot_samples/01.DLL/x64/MiniBotAlgorithmLib.x64.so"
lib_name = "MiniBotAlgorithmLib.x64.so"
dir_path = os.path.dirname(os.path.realpath(__file__)) + "/../"
folder_name = "01.DLL/x64/"
name_real_path = dir_path +  folder_name + lib_name
# udp_lib = cdll.LoadLibrary(dir_path+folder_name+"/"+udp_lib_name)

class MiniBotAlgorithmLib:
    def __init__(self):
        self.lib = cdll.LoadLibrary(name_real_path)

    # 计算关节角度To末端笛卡尔空间
    # 参数： j[5] 五元素的数组指针对于I T T I T (G) 的关节角度值 单位是弧度角 转换为角度单位后值范围
    # I：正负150角度,T：正负90，G：-90到0 （0度的关节模块0位）
    # 输出：笛卡尔空间 X Y Z Roll Pitch Yaw 六个元素的数组
    def CalJointAnglesToSpatialCoordinate(self, angles, count, isF):
        baseHeight = -180.129
        if(isF):
            baseHeight = -224.7
        self.lib.CalForwardKine.restype = POINTER(c_float)
        size = len(angles)
        anglesp = (c_float * size)()
        for i in range(size):
            anglesp[i] = (c_float(angles[i]))
        r = self.lib.CalForwardKine(
            anglesp, c_int(count), c_double(baseHeight))
        xyzRPY = []
        for i in range(6):
            xyzRPY.append(r[i])
        return xyzRPY

    # 计算末端笛卡尔空间To关节角度
    # 参数： pose[6] X Y Z Roll Pitch Yaw 六个元素的数组
    # 输出：关节弧度角 float* 五元素的数组指针对于I T T I T (G) 的关节角度值 单位是弧度角 转换为角度单位后值范围
    # I：正负150角度,T：正负90，G：-90到0 （0度的关节模块0位）
    def CalSpatialCoordinateToJointAngles(self, xyzRPY, count, isF):
        baseHeight = -180.129
        if(isF):
            baseHeight = -224.7
        self.lib.CalInverseKine.restype = POINTER(c_float)
        size = len(xyzRPY)
        xyzRPYp = (c_float * size)()
        for i in range(size):
            xyzRPYp[i] = (c_float(xyzRPY[i]))
        r = self.lib.CalInverseKine(
            xyzRPYp, c_int(count), c_double(baseHeight))

        angles = []
        for i in range(count):
            angles.append(r[i])
        return angles

      # <summary>
      # 添加机器人
      # </summary>
      # <param name="robotType">机器人类别：1遥控小车、2蛇、3双足/双轮、4攀爬、5仿生蜘蛛、6、X轮式</param>
      # <param name="ids">
      # 第一个ID均为与控制底座连接的模块ID，如多个以下分开说明，位置请参考文档说明
      # 攀爬：T:Down,G:Down,T:Up,G:Up
      # 双足/双轮：T:Down,T:Down,IU:Down,T:Up,T:Up,IU:Up
      # </param>
      # <param name="count"></param>
      # <returns>返回机器人控制ID</returns>
    def AddRobot(self, robotType, ids):
        size = len(ids)
        idTypes = c_ubyte * size
        idsp = idTypes()

        #charPP = POINTER(c_char_p)
        #idsValue = c_char_p(size)
        #idsP = charPP(idsValue)
        for i in range(size):
            idsp[i] = (c_ubyte(ids[i]))
        r = self.lib.AddRobot(robotType, idsp, c_int(size))
        return r

    # <summary>
    # 移除机器人
    # </summary>
    # <param name="id">机器Id标识</param>
    # <returns></returns>
    def RemoveRobot(self, id):
        r = self.lib.RemoveRobot(c_int(id))
        return r

    # <summary>
    # 对指定机器人发送指令
    # </summary>
    # <param name="id">机器人ID</param>
    # <param name="cmd">命令类别</param>
    # <param name="parameter">命令参数</param>
    # <returns></returns>
    def SendCmd(self, id, cmd, parameter):
        r = self.lib.SendCmd(c_int(id), c_int(cmd), c_int(parameter))
        return r

    # <summary>
    # 绑定模块控制接口
    # </summary>
    # <param name="setPosition"></param>
    # <param name="setPositionWithSpeed"></param>
    # <param name="setMultiPosition"></param>
    # <param name="setMultiPositionWithSpeed"></param>
    # <param name="setSpeed"></param>
    # <param name="setRunningSpeed"></param>
    # <param name="enableTorque"></param>
    # <param name="setMode"></param>
    # <param name="threadSleep"></param>
    # https://stackoverflow.com/questions/37973033/ctypes-python3-5-oserror-exception-access-violation-writing-0x00000000
    def BindRobotModuleApis(self, id, setPositionCallback, setPositionWithSpeedCallback, setMultiPositionCallback, setMultiPositionWithSpeedCallback, setPositionSpeedCallback, setRunningSpeedCallback, torqueEnableCallback, setModeCallback):

        # 通过此种方式在此处定义，外部调用此方法后，函数地址会被回收，在回调时会引发：exception: access violation reading
        # 0x0000000000000005 异常
        #setPositionCallback_type = CFUNCTYPE(None,c_ubyte, c_double)
        # setPositionWithSpeedCallback_type = CFUNCTYPE(None,c_ubyte,
        # c_double,c_int)
        # setMultiPositionCallback_type = CFUNCTYPE(None,POINTER(c_ubyte),
        # POINTER(c_double),c_int)
        # setMultiPositionWithSpeedCallback_type = CFUNCTYPE(None,POINTER(c_ubyte),
        # POINTER(c_double),c_int , c_int)

        #setSpeedCallback_type = CFUNCTYPE(None,c_ubyte, c_int)
        #setTorqueEnableCallback_type = CFUNCTYPE(None,c_ubyte, c_bool)
        #setModeCallback_type = CFUNCTYPE(None,c_ubyte, c_int)

        #setPosition = setPositionCallback_type(setPositionCallback)
        # setPositionWithSpeed =
        # setPositionWithSpeedCallback_type(setPositionWithSpeedCallback)
        #setMultiPosition = setMultiPositionCallback_type(setMultiPositionCallback)
        # setMultiPositionWithSpeed =
        # setMultiPositionWithSpeedCallback_type(setMultiPositionWithSpeedCallback)

        #setPositionSpeed = setSpeedCallback_type(setPositionSpeedCallback)
        #setRunningSpeed = setSpeedCallback_type(setRunningSpeedCallback)
        #setTorqueEnable = setTorqueEnableCallback_type(torqueEnableCallback)
        #setMode = setModeCallback_type(setModeCallback)
        # 通过此种方式在此处定义，外部调用此方法后，函数地址会被回收，在回调时会引发：exception: access violation reading
        # 0x0000000000000005 异常
        r = self.lib.BindRobotModuleApis(c_int(id), setPositionCallback, setPositionWithSpeedCallback, setMultiPositionCallback,
                                         setMultiPositionWithSpeedCallback, setPositionSpeedCallback, setRunningSpeedCallback, torqueEnableCallback, setModeCallback)
        return r
